#include "Euler_to_R.h"

void Enler_to_R_Matrix(IMU_Angle_t *euler, Matrix_to_R_Typdef *R_out)
{
    float Pitch_PI = euler->Pitch*PI/180.0f;
    float Roll_PI = euler->Roll*PI/180.0f;
    float Yaw_PI = euler->Yaw*PI/180.0f;

    float cosPitch = arm_cos_f32(Pitch_PI);
    float sinPitch = arm_sin_f32(Pitch_PI);
    float cosRoll = arm_cos_f32(Roll_PI);
    float sinRoll = arm_sin_f32(Roll_PI);
    float cosYaw = arm_cos_f32(Yaw_PI);
    float sinYaw = arm_sin_f32(Yaw_PI);

    R_out->Matrix_to_R[0] = cosYaw*cosPitch;
    R_out->Matrix_to_R[1] = cosYaw*sinPitch*sinRoll - sinYaw*cosRoll;
    R_out->Matrix_to_R[2] = cosYaw*sinPitch*cosRoll + sinYaw*sinRoll;

    R_out->Matrix_to_R[3] = sinYaw*cosPitch;
    R_out->Matrix_to_R[4] = sinYaw*sinPitch*sinRoll + cosYaw*cosRoll;
    R_out->Matrix_to_R[5] = sinYaw*sinPitch*cosRoll - cosYaw*sinRoll;

    R_out->Matrix_to_R[6] = -sinPitch;
    R_out->Matrix_to_R[7] = cosPitch*sinRoll;
    R_out->Matrix_to_R[8]= cosPitch*cosRoll;
}
